#include "plan.h"

JointPlanner_TypeDef *joint_planner_new(void)
{
    return (JointPlanner_TypeDef *)calloc(1, sizeof(JointPlanner_TypeDef));
}

int joint_planner_init(float time_lim, float vel_lim, float pos_start, float pos_end, JointPlanner_TypeDef *planner)
{
    LOG_DEBUG("Five Poly Offline Joint Planning...");

    if ((time_lim <= 0 && vel_lim <= 0))
    {
        LOG_ERROR("***Plan Failed!!! Velocity Limit and Time Limit invalid, time_lim = %f, vel_lim = %f ***", time_lim,
               vel_lim);
        return E_PLAN_PARA_INVALID;
    }

    if (fabsf(pos_start - pos_end) < PLAN_EPS)
    {
        LOG_WARN("***Plan Warning!!! Trajectory Start and Stop too near! "
               "start_point = %f, end_point = %f ***",
               pos_start, pos_end);
//        return E_PLAN_POS_TOO_CLOSE;
    }
    planner->pos_start = pos_start;
    planner->pos_end = pos_end;
    float pos_dist = pos_end - pos_start;
    float time;
    if (time_lim > 0)
    {
        //! 若设置了时间条件，则忽略设置的速度条件
        time = time_lim;
        LOG_DEBUG("Planner will plan within Time Limit, whole trajectory cost time: %f seconds.", time);
    }
    else
    {
        time = 15.0f / 8.0f * fabsf(pos_dist) / vel_lim;
        LOG_DEBUG("Planner will plan within Velocity Limit, whole trajectory cost time: %f seconds.", time);
    }
    planner->time_lim = time;
    planner->poly_const[0] = pos_start;
    planner->poly_const[1] = 0.0f;
    planner->poly_const[2] = 0.0f;
    planner->poly_const[3] = (10.0f * pos_dist) / (time * time * time);
    planner->poly_const[4] = (-15.0f * pos_dist) / (time * time * time * time);
    planner->poly_const[5] = (6.0f * pos_dist) / (time * time * time * time * time);

    LOG_DEBUG("Plan Success! Planner Five Poly Parameter: %f, %f, %f, %f, %f, %f", planner->poly_const[0], planner->poly_const[1],
              planner->poly_const[2], planner->poly_const[3], planner->poly_const[4], planner->poly_const[5]);
    return E_PLAN_NO_ERROR;
}

float joint_plan_move(JointPlanner_TypeDef *planner, float t)
{
    if (t <= planner->time_lim)
    {
        planner->pos_cur = planner->poly_const[0] + planner->poly_const[1] * t + planner->poly_const[2] * t * t +
                           planner->poly_const[3] * t * t * t + planner->poly_const[4] * t * t * t * t +
                           planner->poly_const[5] * t * t * t * t * t;
    }
    else
    {
        planner->pos_cur = planner->pos_end;
    }
    return planner->pos_cur;
}
